﻿using System.Collections;
using System.Collections.Generic;
using System.Linq;
using Uniblocks;
using UnityEngine;

public class FindPathUtl : MonoBehaviour
{
    public static void FindPathAir(Vector3 from, Vector3 to, List<Vector3> path)
    {
        int n = 0;
        //path = new List<Vector3>();
        path.Clear();
        from = new Vector3(Mathf.Round(from.x), Mathf.Round(from.y), Mathf.Round(from.z));
        to = new Vector3(Mathf.Round(to.x), Mathf.Round(to.y), Mathf.Round(to.z));
        List<PathNode> openList = new List<PathNode>();
        List<PathNode> closeList = new List<PathNode>();

        bool canMove = Engine.PositionToVoxelInfo(from).GetVoxelType().VColliderType == ColliderType.none;
        if (!canMove)
        {
            return;
        }
        openList.Add(new PathNode(from, 0, Vector3.Distance(from, to), null));
        while (openList.Count > 0)
        {
            n++;
            if (n > 2000)
            {
                return;
            }
            //遍历开启列表，找到消费最小的点作为检查点
            PathNode cur = openList[0];
            for (int i = 0; i < openList.Count; i++)
            {
                if (openList[i].f < cur.f)// && openList[i].h < cur.h)
                {
                    cur = openList[i];
                }
            }
            openList.Remove(cur);
            closeList.Add(cur);
            //检查是否找到终点

            if (cur.pos == to)
            {
                Debug.Log("寻路结束!" + n);
                CreatePath(cur, path);
                path.Reverse();
                return;
            }
            for (int i = -1; i < 2; i++)
            {
                for (int j = -1; j < 2; j++)
                {
                    for (int k = -1; k < 2; k++)
                    {
                        Vector3 temp = new Vector3(i, j, k);
                        PathNode pn = new PathNode(cur.pos + temp, cur.g + 1, Vector3.Distance(cur.pos + temp, to), cur);
                        if (temp.magnitude == 0 || openList.Contains(pn) || closeList.Contains(pn) || Vector3.Distance(from, cur.pos + temp) > 64)
                        {
                            continue;

                        }
                        VoxelInfo vi = Engine.PositionToVoxelInfo(cur.pos + temp);
                        Voxel v = vi.GetVoxelType();
                        if (v.VColliderType == ColliderType.none)
                        {

                            openList.Add(pn);
                        }
                    }
                }
            }
        }

    }

    public static float PDis(Vector3 p1, Vector3 p2)
    {
        List<float> dis = new List<float>();
        dis.Add(Mathf.Abs(p1.x - p2.x));
        dis.Add(Mathf.Abs(p1.y - p2.y));
        dis.Add(Mathf.Abs(p1.z - p2.z));
        return dis.Max();
    }

    public static void CreatePath(PathNode endNode, List<Vector3> path)
    {
        path.Add(endNode.pos);
        if (endNode.lastNode != null)
        {
            CreatePath(endNode.lastNode, path);
        }
    }
}

public class PathNode
{
    public Vector3 pos;
    public float g;
    public float h;
    public float f;
    public PathNode lastNode;
    public PathNode(Vector3 pos, float g, float h, PathNode lastNode)
    {
        this.pos = pos;
        this.g = g;
        this.h = h;
        f = g + h;
        this.lastNode = lastNode;
    }
    public override bool Equals(object obj)
    {
        if (obj is PathNode)
        {
            PathNode pn = obj as PathNode;
            return pn.pos == pos;
        }
        return base.Equals(obj);
    }

    public override int GetHashCode()
    {
        return pos.GetHashCode();
    }
}
